package com.hitqz.robot.driver;


import cn.hutool.core.util.StrUtil;
import cn.hutool.json.JSONUtil;
import com.google.common.collect.Lists;
import com.hitqz.robot.api.business.enums.*;
import com.hitqz.robot.api.common.dto.form.FormConfigDto;
import com.hitqz.robot.api.common.dto.form.FormFieldConfigDto;
import com.hitqz.robot.api.common.dto.robot.InitPosDto;
import com.hitqz.robot.api.common.dto.robot.RobotGoTargetPosDto;
import com.hitqz.robot.api.common.dto.robot.RobotMoveDto;
import com.hitqz.robot.api.common.entity.Robot;
import com.hitqz.robot.api.common.entity.RobotMap;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.common.core.util.R;
import com.hitqz.robot.driver.dto.KcRobotRunningStatus;
import com.hitqz.robot.driver.dto.robot.RobotCustomInfo;
import com.hitqz.robot.driver.model.IRobotPosDto;
import com.hitqz.robot.driver.model.MapAttributesDto;
import com.hitqz.robot.driver.model.PointDto;
import com.hitqz.robot.driver.model.RobotIOStatusDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.hitqz.robot.driver.util.DataTypeEnum;
import com.hitqz.robot.driver.util.KcCacheUtil;
import com.hitqz.robot.driver.util.KcToolkit;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import java.util.*;

/**
 * @author wengyx
 * @date 2023/8/20
 */
@Slf4j
@Service("robotDriverService")
public class KcRobotDriverImpl implements RobotDriverService {

    private static RobotMap map;

    private static float speed = 0.2f;

    private static float minSpeed = 0.05f;

    private static float maxSpeed = 1.2f;

    /**
     * 创建科聪udp客户端
     * @param robot 机器人实体类
     * @return true/false
     */
    @Override
    public boolean doOnlineMode(Robot robot) {
        boolean result = false;
        try {
            KcToolkit.robot = robot;
            result = KcToolkit.init(robot.getIpAddress());
        }catch (Exception e){
            log.error("[kc] doOnlineMode error",e);
        }
        log.info("[kc] doOnlineMode success");
        return result;
    }

    @Override
    public boolean setSpeed(RobotMoveDto baseSpeedDto) {
        String linerXSpeed = String.valueOf(speed);
        String linerYSpeed = String.valueOf(speed);
        String angularSpeed = "0.2";
        boolean lineXResult=true;
        boolean lineYResult=true;
        boolean angleResult = true;
        if (StrUtil.isNotBlank(baseSpeedDto.getLinerSpeed()) && Objects.equals(baseSpeedDto.getLinerSpeedY(),"0")) {
            //KcCacheUtil.setSpeedFlag();
            float webSpeed = Math.abs(Float.valueOf(baseSpeedDto.getLinerSpeed()));
            linerXSpeed = String.valueOf(Math.min(Float.parseFloat(linerXSpeed),webSpeed));
            linerXSpeed = baseSpeedDto.getLinerSpeed().contains("-") ? "-" + linerXSpeed :  linerXSpeed;
            linerXSpeed = Objects.equals(baseSpeedDto.getLinerSpeed(),"0") ? "0" : linerXSpeed;
            lineXResult = KcToolkit.writeVar("cmd_vel_line_x", DataTypeEnum.REAL, linerXSpeed, false);
            log.info("[kc] set line x speed {} m/s",linerXSpeed);
        }
        if (StrUtil.isNotBlank(baseSpeedDto.getLinerSpeedY()) && Objects.equals(baseSpeedDto.getLinerSpeed(),"0")){
            KcCacheUtil.setSpeedFlag();
            linerYSpeed = baseSpeedDto.getLinerSpeedY().contains("-") ? "-" + linerYSpeed :  linerYSpeed;
            linerYSpeed = Objects.equals(baseSpeedDto.getLinerSpeedY(),"0") ? "0" : linerYSpeed;
            lineYResult = KcToolkit.writeVar("cmd_vel_line_y", DataTypeEnum.REAL, linerYSpeed, false);
            log.info("[kc] set line y speed {} m/s",linerYSpeed);
        }
        if (StrUtil.isNotBlank(baseSpeedDto.getAngularSpeed())) {
            KcCacheUtil.setSpeedFlag();
            float webSpeed = Math.abs(Float.valueOf(baseSpeedDto.getAngularSpeed()));
            angularSpeed = String.valueOf(Math.min(Float.parseFloat(angularSpeed),webSpeed));
            angularSpeed = baseSpeedDto.getAngularSpeed().contains("-") ? "-" + angularSpeed :  angularSpeed;
            angleResult = KcToolkit.writeVar("cmd_vel_angle", DataTypeEnum.REAL, angularSpeed, false);
            log.info("[kc] set angle speed {} m/s",angularSpeed);
        } else if (Objects.equals(baseSpeedDto.getLinerSpeed(),"0")
                && Objects.equals(baseSpeedDto.getAngularSpeed(), "0")
                && Objects.equals(baseSpeedDto.getLinerSpeedY(), "0")
        ) {
            // 切换上位机模式
            boolean result = KcToolkit.writeVar("cmd_forcedctrl", DataTypeEnum.UDINT, "0", true);
            log.info("[kc] 切换自动模式 {}",result);
            KcToolkit.writeVar("cmd_vel_line_x",DataTypeEnum.REAL,"0.0", false);
            KcToolkit.writeVar("cmd_vel_line_y",DataTypeEnum.REAL,"0.0",false);
            KcToolkit.writeVar("cmd_vel_angle",DataTypeEnum.REAL,"0.0",false);

        }
        return lineXResult & lineYResult & angleResult;
    }

    @Override
    public boolean changeWebControlSpeed(float changeValue) {
        speed = Math.min(Math.max(speed + changeValue,minSpeed), maxSpeed);
        return true;
    }

    /**
     * 设置初始位置 （web下发的话要先调用canvas2Point再调这个接口）
     * @param baseInitPosDto 激光坐标
     * @return true/false
     */
    @Override
    public boolean setInitPos(InitPosDto baseInitPosDto) {
        float posX = baseInitPosDto.getPosx();
        float posY = baseInitPosDto.getPosy();
        float posYaw = baseInitPosDto.getYaw();
        return KcToolkit.setInitPos(posX, posY, posYaw);
    }

    @Override
    public boolean robotTaskGoTarget(RobotGoTargetPosDto dto) {
        try {
            if(dto.isCharge()) {
                // 判断当前是否在充电 当前充电中-》想要充电不管
                IRobotPosDto posDto = getWebPos();
                if (posDto.isCharging()) {
                    log.info("当前已经在充电中了，不需要再次发送点位");
                    return true;
                }
            } else {
                // 当前不是充电-》 循环判断取消充电
                // 写入取消充电
                while (true) {
                    KcToolkit.writeVar("cmd_a_charge", DataTypeEnum.INT, "0", false);
                    IRobotPosDto posDto = getWebPos();
                    if (!posDto.isCharging()) {
                        break;
                    }
                }
            }
            KcRobotRunningStatus runningStatus = (KcRobotRunningStatus) KcCacheUtil.get(KcCacheUtil.KEY_RUNNING_STATUS);
            if (runningStatus != null && Objects.equals(runningStatus.getRunMode(), 0)) {
                //切换自动模式
                KcToolkit.changeRunningMode(1);
                Thread.sleep(200);
            }
            KcToolkit.navToPosControl(1, "0");
            Thread.sleep(200);
            log.info("[kc]前往目标点: {}", dto.getCode());
            //if ()
            return KcToolkit.navToPosControl(0, dto.getCode());
        }catch (Exception e){
            log.info("[kc]前往目标点失败:{}",e.getMessage());
            return false;
        }
    }


    /**
     * 中断机器人的动作
     * @return
     */
    @Override
    public boolean motionAbort() {
        // 通知调度软件
        KcToolkit.navToPosControl(1, "0");
        KcToolkit.writeVar("set_Dispathing", DataTypeEnum.INT, String.valueOf("0"), false);
        return true;
    }

    @Override
    public boolean navCmd(Integer cmd) {
        boolean result = false;
        if (CmdNavStatusEnum.NAV_PAUSE.getCode() ==cmd) {
            result = KcToolkit.navToPosControl(2, null);
        } else if (CmdNavStatusEnum.NAV_CANCEL.getCode() == cmd) {
            result = KcToolkit.navToPosControl(1, null);
        } else if (CmdNavStatusEnum.NAV_CONTINUE.getCode() == cmd) {
            result = KcToolkit.navToPosControl(3, null);
        }
        return result;
    }

    @Override
    public boolean cmdCharge(Integer cmd) {
        return KcToolkit.writeVar("cmd_a_charge", DataTypeEnum.INT,String.valueOf(cmd), true);
    }

    @Override
    public R executeOtherCommand(RobotOtherCommandEnum cmdEnum,String jsonParams) {
        switch (cmdEnum) {
            case AVOID_SWITCH -> {
                // o 开 1-关
                KcToolkit.writeVar("obs_switch",DataTypeEnum.BOOL,jsonParams, false);
                if (jsonParams.equals("1")) {
                    KcToolkit.writeVar("play_voice",DataTypeEnum.INT,"0", false);
                }
                break;
            }
            case ROD_CONTROLLER -> {
                boolean result = KcToolkit.writeVar("cmd_forcedctrl", DataTypeEnum.UDINT, "1", false);
                log.info("[kc] other Command切换上位机模式{}",result);
                break;
            }
            case JACKING_UP -> {
                //KcToolkit.writeVar("cmd_lifter",DataTypeEnum.INT, "1", false);
                KcToolkit.writeVar("cmd_lifter",DataTypeEnum.INT,String.valueOf(1), false);
                KcToolkit.writeVar("cmd_lifter_f",DataTypeEnum.INT,String.valueOf(1), false);
                KcToolkit.writeVar("cmd_lifter_b",DataTypeEnum.INT,String.valueOf(1), false);
                break;
            }
            case JACKING_DOWN -> {
                //KcToolkit.writeVar("cmd_lifter",DataTypeEnum.INT, "-1", false);
                KcToolkit.writeVar("cmd_lifter",DataTypeEnum.INT,String.valueOf(-1), false);
                KcToolkit.writeVar("cmd_lifter_f",DataTypeEnum.INT,String.valueOf(-1), false);
                //KcToolkit.writeVar("cmd_lifter_b",DataTypeEnum.INT,String.valueOf(-1), false);
                break;
            }
            case JACKING_STATUS -> {
                Integer result = 0;
                try {
                    int front = Integer.parseInt(KcToolkit.readVar("rep_f_l_status"));
                    //int back = Integer.parseInt(KcToolkit.readVar("rep_f_l_status"));
                    result = front ;
                } catch (Exception e) {
                }
                //return R.ok(Integer.parseInt(Optional.ofNullable(KcToolkit.readVar("rep_b_l_status")).orElse("0")));
                return R.ok(result);
            }
            case WRITE_VAR -> {
                Map<String, Object> map = JSONUtil.parseObj(jsonParams);
                String varName = (String) map.get("varName");
                String varType = (String) map.get("varType");
                String value = (String) map.get("varValue");
                log.info("计算写入变量{}---{}--{}", varName, varType, value);
                KcToolkit.writeVar(varName,  DataTypeEnum.valueOf(varType), value, false);
            }
            case READ_VAR -> {
                Map<String, Object> map = JSONUtil.parseObj(jsonParams);
                String varName = (String) map.get("varName");
                String result = KcCacheUtil.getVarCache(varName);
                return R.ok(result);
            }
            default -> {
                //log.info("Unknown command:" + cmdEnum.getKcCommand());
                break;
            }
        }
        return null;
    }

    /**
     * 获取地图显示坐标
     * @return
     */
    @Override
    public IRobotPosDto getWebPos() {

        KcRobotRunningStatus runningStatus =(KcRobotRunningStatus) KcCacheUtil.get(KcCacheUtil.KEY_RUNNING_STATUS);
        if (runningStatus==null){
            runningStatus = new KcRobotRunningStatus();
        }
        IRobotPosDto iRobotPosDto = KcRobotRunningStatus.getIRobotPosDto(runningStatus);
        iRobotPosDto.setUrl(Optional.ofNullable(map).map(RobotMap::getFilePath).orElse(""));
        iRobotPosDto.setCurrentMap(Optional.ofNullable(map).map(RobotMap::getMapCode).orElse(""));
        iRobotPosDto.setWebControlSpeed(speed);
        iRobotPosDto.setCurrentDirection(runningStatus.getIsCharging()== 0 ? "放电": "充电");
        //坐标转换
        try {
            if (map != null && map.getHeight() != null && map.getWidth() != null) {
                PointDto pointDto = point2Canvas(map, new PointDto(iRobotPosDto.getX(), iRobotPosDto.getY()));
                iRobotPosDto.setPixelX(Double.valueOf(pointDto.getX()).intValue());
                iRobotPosDto.setPixelY(Double.valueOf(pointDto.getY()).intValue());
                iRobotPosDto.setPixelAngle(iRobotPosDto.getAngle());
            }
            ArrayList<PointDto> pointDtos = new ArrayList<>();
            //iRobotPosDto.setQrCodeX(KcToolkit.readVar("rep_Cam_X"));
            //iRobotPosDto.setQrCodeY(KcToolkit.readVar("rep_Cam_Y"));
            //iRobotPosDto.setQrCodeZ(KcToolkit.readVar("rep_Cam_Z"));
            // TODO: 形式过的路径
            iRobotPosDto.setPassedPath(pointDtos);
            List<RobotIOStatusDto> ioStatusDtos = Lists.newArrayList();
            // 双轮对   前 io： 3 后 ：4
            RobotIOStatusDto statusDto = new RobotIOStatusDto();
            statusDto.setId(3);
            //statusDto.setStatus(KcToolkit.readVar("rep_front_io").equals("1"));
            statusDto.setStatus("1".equals(KcCacheUtil.getVarCache("rep_front_io")));
            ioStatusDtos.add(statusDto);
            statusDto = new RobotIOStatusDto();
            statusDto.setId(4);
            //statusDto.setStatus(KcToolkit.readVar("rep_back_io").equals("1"));
            statusDto.setStatus("1".equals(KcCacheUtil.getVarCache("rep_back_io")));
            ioStatusDtos.add(statusDto);
            iRobotPosDto.setDI(ioStatusDtos);
            //String upIo = KcToolkit.readVar("rep_up_io");
            String upIo = KcCacheUtil.getVarCache("rep_up_io");
            statusDto = new RobotIOStatusDto();
            statusDto.setId(0);
            statusDto.setStatus(("1").equals(upIo));
            ioStatusDtos.add(statusDto);
            iRobotPosDto.setDI(ioStatusDtos);

            // 双轮对-前顶升
            statusDto = new RobotIOStatusDto();
            statusDto.setId(1);
            statusDto.setStatus("1".equals(KcCacheUtil.getVarCache("rep_up_io_f")));
            ioStatusDtos.add(statusDto);

            // 双轮-后传感器
            statusDto = new RobotIOStatusDto();
            statusDto.setId(2);
            statusDto.setStatus("1".equals(KcCacheUtil.getVarCache("rep_up_io_b")));
            ioStatusDtos.add(statusDto);

            // 用顶升状态
            String fullStatus = KcCacheUtil.getVarCache("rep_up_io_f");
            iRobotPosDto.setFull(("1").equals(fullStatus));
        }catch (Exception e){
            //log.warn("[kc]坐标转换错误{}",e);
        }
        //导航信息
        String robotStatus = String.valueOf(KcCacheUtil.get(KcCacheUtil.KEY_ROBOT_RUNNING_STATUS));
        String  currentPath= KcCacheUtil.getVarCache("repCurPathPoint");
        String  currentPoint= KcCacheUtil.getVarCache("repNaviGoalID");
        String navStatus = KcCacheUtil.getVarCache("repNaviStatus");
        boolean reachable = false;
        String currentStation = currentPoint;
        if (StrUtil.isNotEmpty(navStatus) && StrUtil.isNotEmpty(currentPoint) && StrUtil.isNotEmpty(currentPath)) {
            int pointId = Integer.parseInt(currentPoint);
            int nav = Integer.parseInt(navStatus);
            if (pointId == 0) {
                currentStation = currentPath;
            }
            if (pointId > 0 && nav == 4) {
                reachable = true;
            }
            if (pointId == 0 && nav == 0) {
                reachable = true;
            }
        }
        iRobotPosDto.setReached(reachable);
        iRobotPosDto.setCurrentStation(currentStation);
        iRobotPosDto.setLastStation(currentStation);
        ///  双轮对

        return iRobotPosDto;
    }

    /**
     * 获取路径   注：科聪使用不指定路径方式导航，只要下发目的，让控制器自己规划就好
     * @param sourcePos 起点点位
     * @param targetPos 目标点位
     * @param allRobotMapPos 所有地图点
     * @return 路径点位列表
     */
    @Override
    public List<RobotMapPos> getNavPath(RobotMapPos sourcePos, RobotMapPos targetPos, List<RobotMapPos> allRobotMapPos) {
        // 路径直接通过最后一个点
        List<RobotMapPos> robotMapPosList = new ArrayList<>();
        robotMapPosList.add(targetPos);
        return robotMapPosList;
    }

    @Override
    public List<RobotMapPos> getRobotMapPos(String mapCode) {
        List<RobotMapPos> list = KcToolkit.parsePosInfo();
        if (list==null||list.isEmpty()){
            return List.of();
        }
        return list;
    }

    @Override
    public List<RobotMap> getMapList() {
        RobotMap robotMap = KcToolkit.parseMapInfo();
        if (robotMap == null) {
            return List.of();
        }
        return List.of(robotMap);
    }

    @Override
    public String getCurrentMap() {
        KcRobotRunningStatus kcRobotRunningStatus = KcToolkit.queryRunningStatus();
        return kcRobotRunningStatus.getCurrentMapName();
    }

    @Override
    public MapAttributesDto getMapAttributes(String mapCode) {
        RobotMap robotMap = KcToolkit.parseMapInfo();
        if (robotMap==null){
            return null;
        }
        MapAttributesDto dto = new MapAttributesDto();
        dto.setCurrentMapName(robotMap.getMapCode());
        dto.setMapOriginx(robotMap.getMapOriginx());
        dto.setMapOriginy(robotMap.getMapOriginy());
        dto.setMapResolution(robotMap.getMapResolution());
        return dto;
    }

    @Override
    public void selMap(RobotMap map) {
        KcRobotDriverImpl.map = map;
        log.info("选择地图:{}",map);
    }

    @Override
    public PointDto canvas2Point(RobotMap robotMap, PointDto pointDto) {
        return null;
    }

    @Override
    public FormConfigDto getRobotConfig(String formName) {
        // 创建字段配置
        ArrayList<FormFieldConfigDto> fields = new ArrayList<>();
        //传感器使能
        fields.add(RobotParamsFormFieldEnum.NAV_LIDAR_ENABLED.toConfigDto());
        // 464模块配置 读气体传感器
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_IP.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM1_PORT.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM1_NAME.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM1_MESSAGE_TEXT.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM2_PORT.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM2_NAME.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM2_MESSAGE_TEXT.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM3_PORT.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM3_NAME.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM3_MESSAGE_TEXT.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM4_PORT.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM4_NAME.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.TAS_LAN_464_COM4_MESSAGE_TEXT.toConfigDto());
        // 开门器
        fields.add(RobotParamsFormFieldEnum.M518R_COM_NAME.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.M518R_COM_OPEN_MESSAGE_TEXT.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.M518R_COM_CLOSE_MESSAGE_TEXT.toConfigDto());
        // 通过 FormConfigEnum 创建表单配置
        return FormConfigEnum.ROBOT_PARAMS_CONFIG.toFormConfigDto(fields);
    }

    @Override
    public RobotDriverEnum getDriverVersion() {
        return RobotDriverEnum.KC;
    }

    private PointDto point2Canvas(RobotMap robotMap, PointDto pointDto){
        PointDto pixelPoint = new PointDto();
        pixelPoint.setX((pointDto.getX()-robotMap.getMinX())*robotMap.getWidth()/(robotMap.getMaxX()-robotMap.getMinX()));
        pixelPoint.setY((robotMap.getMaxY()-pointDto.getY())*robotMap.getHeight()/(robotMap.getMaxY()-robotMap.getMinY()));
        return pixelPoint;
    }

    @Override
    public List<RobotCustomInfo> getCustomInfo() {
        List<RobotCustomInfo> robotCustomInfos = Lists.newArrayList();
        robotCustomInfos.add(RobotCustomInfo.create("camp_X", "二维码X", KcToolkit.readVar("rep_Cam_X")));
        robotCustomInfos.add(RobotCustomInfo.create("camp_Y", "二维码Y", KcToolkit.readVar("rep_Cam_Y")));
        robotCustomInfos.add(RobotCustomInfo.create("camp_Z", "二维码Z", KcToolkit.readVar("rep_Cam_Z")));
        return robotCustomInfos;
    }


    @Override
    public boolean changeDispatch(Integer onlineStatus) {
        // 写入变量 调度上线
        if (onlineStatus == 0) {
            log.info("机器人调度下线");
            KcToolkit.writeVar("rep_DispOLCtl_C", DataTypeEnum.INT, "2", false);
        } else if (onlineStatus == 1) {
            log.info("机器人调度上线");
            KcToolkit.writeVar("rep_DispOLCtl_C", DataTypeEnum.INT, "1", false);
        }
        return true;
    }
}
